#include "costmap_publish.h"


namespace costmap_2d{

Costmap2DPublisher::Costmap2DPublisher(Costmap2D* costmap, std::string global_frame, std::string topic_name):
    costmap_(costmap), global_frame_(global_frame){
    costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(topic_name, 1);

    if(cost_translation_table_ == NULL){
        cost_translation_table_ = new char[256];
    
        // special values:
        cost_translation_table_[0] = 0;  // NO obstacle
        cost_translation_table_[253] = 99;  // INSCRIBED obstacle
        cost_translation_table_[254] = 100;  // LETHAL obstacle
        cost_translation_table_[255] = -1;  // UNKNOWN
    
        // regular cost values scale the range 1 to 252 (inclusive) to fit
        // into 1 to 98 (inclusive).
        for (int i = 1; i < 253; i++){
            cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
        }
    }

}

void Costmap2DPublisher::publishCostmap(){
    if (costmap_pub_.getNumSubscribers() == 0){
        return;
    }    

    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
    
    nav_msgs::OccupancyGrid grid_;
    grid_.header.frame_id = global_frame_;
    grid_.header.stamp = ros::Time::now();
    grid_.info.resolution = costmap_->getResolution();;
    grid_.info.width = costmap_->getWidth();
    grid_.info.height = costmap_->getHeight();

    double wx, wy;
    costmap_->mapToWorld(0, 0, wx, wy);
    grid_.info.origin.position.x = wx - grid_.info.resolution / 2;
    grid_.info.origin.position.y = wy - grid_.info.resolution / 2;
    grid_.info.origin.position.z = 0.0;
    grid_.info.origin.orientation.w = 1.0;

    grid_.data.resize(grid_.info.width * grid_.info.height);
    unsigned char* data = costmap_->getCharMap();
    for (unsigned int i = 0; i < grid_.data.size(); i++){
      grid_.data[i] = cost_translation_table_[ data[ i ]];
    }

    costmap_pub_.publish(grid_);
}


}   // namespace costmap_2d